1. Field of the Invention
The present invention relates to a position detecting apparatus and method used in a navigation system, and more specifically to a position detecting apparatus and method used in a navigation system which correct a state quantity measured with a dead reckoning navigation unit (a state quantity including a vehicle position, a vehicle speed, and an attitude of a sensor or a vehicle) at a predetermined periodic interval and then recalculate a state quantity based on the corrected state quantity and output the calculated state quantity with the dead reckoning navigation unit.
2. Description of the Related Art
Dead reckoning navigation systems are configured using a vehicle speed pulse sensor as a vehicle distance sensor and an inertial sensor such as an accelerometer or an angular velocity meter (gyroscope) and are widely used to estimate a position or speed of a moving vehicle, an attitude of a sensor, or an attitude of a vehicle.
A general INS (inertial navigation system) includes a triaxial accelerometer and a triaxial gyroscope, and is used to determine six degrees of freedom of a moving object, in other words, determine both of three-dimensional translational motion and three-dimensional rotation. For example, a navigation system of an airplane or spacecraft determines six degrees of freedom of a moving object, that is, both three-dimensional translational motion and three-dimensional rotation using such an inertial navigation system. As for an in-vehicle navigation system, a vehicle speed pulse sensor is often used in place of an inertial sensor that is more costly.
According to dead reckoning navigation, a determined position can be updated based on sensor output without relying on an external signal. However, noise or other such error included in the sensor output increases over time at each integral calculation and thus, navigation is inadequate for long-time use. To that end, the following method has been devised (Japanese Unexamined Patent Application Publication No. 8-68655). The method integrates a GPS (global positioning system) receiver that provides a determined position with a finite error when there is reception of a satellite signal with a dead reckoning navigation system using an optimum probability algorithm such as a Kalman filter to thereby complementarily overcome or solve problems in the performance of both of the systems. Such a system is called an “integrated navigation system” and today finds wide use in applications from vehicle navigation to aerospace navigation.
The Kalman filter processing is intended to correct an error between a predicted (estimated) value of a predetermined state quantity and an observation value at a predetermined time to sequentially obtain an optimum estimated value at a corresponding time. In the Kalman filter processing, a calculating expression for estimating a target value is set in advance, and the estimation process is repeated until a time n at which an observation value Z(t) is obtained based on the calculation expression. If the observation value is obtained at the time n, an estimated value correction calculation is carried out to minimize a probabilistically defined error between the observation value and the estimated value at the time n.
FIG. 14 is a schematic explanatory view of Kalman filter processing. The Kalman filter processing is divided into a signal generation process 1 and an observation process 2 as shown in FIG. 14. In FIG. 14, a linear system F is provided. If a state of the linear system is represented by X(t) and a part of X(t) can be derived from an observation matrix H, the Kalman filter gives an optimum estimated value of X(t). Here, w represents noise generated in the signal generation process, and v represents noise generated in the observation process. The Kalman filter processing repeats Kalman processing at a predetermined periodic interval while using an input value as the observation value Z(t) to thereby obtain the optimum estimated value X(t).
A state equation of a system model in the Kalman filter processing is as follows:
[Expression 1]δX(k+1)=F(k)δX(k)+w(k)  (1)
A system state variable δX includes a state quantity such as each axial velocity, each axial position, and a sensor attitude, and is expressed as follows:
[Expression 2]δx=[δνxs, δνys, δνzs, δN, δE, δD, δα, δβ, δγ, bωxs, bωys, bωzs, baxs, baysbazs, δb, δc, δkωx, δkωy, δkωz]  (2)
The calculation is performed based on Expression 1 at a periodic interval T that is shorter than an interval at which the observation value Z(t) is obtained, and the obtained value is output as a state quantity. Then, if the observation value Z(t) is obtained, the optimum estimated value X(t) is calculated through Kalman filter processing. From then on, the calculation is performed using the optimum estimated value based on Expression 1.
An observation equation of the Kalman filter is as follows:
[Expression 3]δZ(k)=H(k)δX(k)+v(k)  (3)
An error of the system state variable derived from Expression 1 is accumulated with time. To that end, the optimum estimated value of a system state variable δX(k) is determined each time an observation value δAZ(k) is obtained. From then on, the system state quantity δX(k) is calculated using the optimum estimated value and output based on Expression 1 until the next observation value is obtained.
To be specific, the Kalman filter executes a calculation based on Expression 4 below to obtain the optimum estimated value X(t|t)(=δX(t|t)) each time the observation value Z(t)(=δZ(t)) is obtained (at a periodic interval at which the observation value δZ(t) is input). Here, it is assumed that an estimated value of A, which is obtained at a time i based on information up to a time j, is expressed by A(i|j). In the following expression, X(t|t−1) represents a previously estimated value, and K(t) represents a Kalman gain.
[Expression 4]X(t|t)=X(t|t−1)+K(t){Z(t)−HX(t|t−1)}  (4)
The optimum estimated value X(t|t) is calculated by multiplying a difference in brackets { } on the right side by the Kalman gain. The previously estimated value and the Kalman gain are expressed as follows:
[Expression 5]X(t|t−1)=FX(t−1|t−1)  (5a)K(t)=P(t|t−1)HT(HP(t|t−1)HT+V)−1  (5b)
The previously estimated value X(t|t−1) is updated based on Expression 5a corresponding to Expression 1 at a period interval shorter than the period interval at which Z(t) is input.
Further, in Expression 5b, P represents an error covariance matrix of the state quantity Z, P(t|t−1) represents a predicated value of error covariance, and P(t−1|t−1) represents an error covariance. P(t|t−1) and P(t−1|t−1) are expressed as follows:
[Expression 6]P(t|t−1)=FP(t−1|t−1)FT+W) P(t−1|t−1)=(I−K(t−1)H)P(t−1|t−2)  (6)
V represents a variance of noise v generated in the observation process, and W represents a variance of noise w generated in the signal generation process. A suffix ( )T means a transposed matrix, and a suffix ( )−1 means an inverse matrix. Further, “I” refers to a unit matrix. Moreover, V and W represent white Gaussian noise with zero mean and have no correlation. In such a Kalman filter, if an appropriate error is set for an initial value of the error covariance P and the state quantity X, and an optimum estimated value is obtained based on Expression 4 upon each observation and from then on, the state quantity X is calculated based on Expression 5a, and an accuracy of the state quantity can be improved.
FIG. 15 is a diagram of a position detecting unit in the in-vehicle navigation system described by the assignee of the subject invention in Japanese Unexamined Patent Application Publication No. 2008-039563. Dead-reckoning input signals are classified into a signal output from an inertial sensor on a sensor board 10 and a vehicle speed pulse obtained from a vehicle body through a different cable. As the inertial sensor, an accelerometer (triaxial one for detecting an acceleration) 10a and a gyroscope (triaxial one for detecting an angular velocity) 10b are adopted. As the sensor for generating a vehicle speed pulse, a vehicle speed sensor 11 that generates one pulse each time a vehicle moves a predetermined distance is employed. FIG. 16A shows a sensor coordinate system Xs-Ys-Zs fixed to the sensor board 10. In the following Expressions, it should be noted that a value with a suffix “s” is related to the sensor (board) coordinate system. FIG. 16B shows the sensor configuration of a basic system having six degrees of freedom with three axes (Acc x, Acc y, Acc z) of the accelerometer 10a and three axes (Gyro x, Gyro y, Gyro z) of the gyroscope 10b on the sensor board 10. The triaxial accelerometer (Acc x to Acc z) detects acceleration in three coordinate directions (x, y, z) of the sensor coordinate system, and the triaxial gyroscope (Gyro x to Gyro z) detects angular velocities P, Q, and R about the three coordinate axes (x, y, z) of the sensor coordinate system.
FIG. 17A shows a coordinate system Xb-Yb-Zb fixed to a vehicle. In the following Expressions, a value with a suffix “b” is related to the vehicle-fixed coordinate system. FIG. 17B shows a surface fixed coordinate system North-East-Down (NED coordinate system) at any latitude and longitude. In the following Expressions, a value with a suffix “n” is related to the NED coordinate system.
Returning to FIG. 15, a dead reckoning calculating unit 12 updates and outputs a vehicle state quantity inclusive of current vehicle position, speed, and attitude based on a predetermined computational expression using an algorithm of a basic system having six degrees of freedom at a high frequency, for example, 2.5 Hz.
A GPS receiver 13 receives, from each of plural artificial satellites, a signal regarding a distance and a rate of change in distance to input measured values of a position (latitude N, longitude E, height D) of an antenna of a vehicle and a vehicle speed (northward speed VN, eastward speed VE, vertical speed VD) to a Kalman filter correcting unit 14 at 1 Hz. The Kalman filter correcting unit 14 corrects a vehicle state quantity inclusive of current vehicle position, speed, and attitude. For example, the Kalman filter correcting unit 14 performs state quantity correction processing using a speed (during moving) output from the vehicle speed sensor 11 or an angular velocity offset (when stopped) output from a gyroscope 10b as an observation value and then inputs the resultant value to the dead reckoning calculating unit 12, at 5 Hz. Further, the Kalman filter correcting unit 14 inputs to the dead reckoning calculating unit 12 a corrected state quantity obtained by performing the state quantity correction processing using the position and speed (N, E, D, vN, vE, vD) output from the GPS receiver 13 at 1 Hz.
FIG. 18 is a flowchart of the processing of the position detecting unit in FIG. 15.
First, initial values of three-dimensional vehicle positions N, E, D, a vehicle speed Vx, a pitch angle θ, a vehicle mounting pitch angle A, a yaw angle Y, a vehicle mounting yaw angle A2, a gyroscope offset ωOF, and an acceleration sensor offset αOF are set in the dead reckoning calculating unit 12 (step S101). The gyroscope 10b and the accelerometer 10a are ideally mounted to a vehicle in parallel to both sides of the vehicle as viewed from the side. However, as shown in FIG. 19A, a mounting error is involved when mounted. Thus, the sensors are mounted at an angle A (mounting pitch angle) to the vehicle direction. Here, an angle θ between the horizontal direction and the sensor direction is referred to as “pitch angle”, and the pitch angle is the sum of an inclined angle and the mounting pitch angle. Further, the gyroscope 10b and the accelerometer 10a are ideally mounted to the vehicle in the form of being collinear with the vehicle direction as projected on a plane. However, a mounting error is involved when mounted. Thus, as shown in FIG. 19B, the sensors are mounted at an angle A2 (mounting yaw angle) to the vehicle direction. An angle Y between the northward direction and the sensor direction is referred to as “yaw angle”. The yaw angle Y is the sum of the vehicle direction and the mounting yaw angle.
Subsequently, the dead reckoning calculating unit 12 receives output signals of the gyroscope 10b, the accelerometer 10a, and the vehicle speed sensor 11 (step S102) and calculates a state quantity such as each axial velocity, each axial position, and a sensor attitude based on Expression 2 following a dead reckoning calculating algorithm at a first frequency (frequency of 25 Hz) and then outputs the resultant value (step S103).
Next, the dead reckoning calculating unit 12 checks whether the frequency is switched to a second frequency (frequency of 10 Hz) (step S104). If the frequency is not switched to the second frequency, the dead reckoning calculating unit 12 repeats step S102 and subsequent steps.
If the frequency is switched to the second frequency, the dead reckoning calculating unit 12 determines whether a vehicle is stopped based on whether the vehicle speed Vx is kept at zero for 2 seconds or more (step S105).
If the vehicle is not stopped, the dead reckoning calculating unit 12 checks whether the frequency is switched to a third frequency (1 Hz-frequency=GPS location frequency) (step S106). If the frequency is not switched to the third frequency, the Kalman filter correcting unit 14 corrects a vehicle longitudinal speed (observation value) Vx calculated using an output signal of the vehicle speed sensor 11 and a vehicle speed calculated with the dead reckoning calculating unit 12 based on Kalman filter processing and inputs the corrected state quantity to the dead reckoning calculating unit 12 (step S107). In step S107, first correction processing is executed using a first Kalman filter observation matrix H1.
In step S106, if the frequency is switched to the third frequency, the Kalman filter correcting unit 14 uses the three-dimensional vehicle positions N, E, D and three-dimensional vehicle speeds vN, vE, vD output from the Kalman filter correcting unit 14 as observation values to correct a vehicle state quantity and then inputs the corrected state quantity to the dead reckoning calculating unit 12 (step S108). In step S108, second correction processing is executed using a second Kalman filter observation matrix H2.
In step S105, if the vehicle is stopped, it is checked whether the frequency is switched to the third frequency (1 Hz-frequency=GPS location frequency) (step S109). If the frequency is not switched to the third frequency, the Kalman filter correcting unit 14 carries out the correction processing in step S107 as well as corrects a vehicle state quantity based on a gyroscope output signal regarding an angular velocity (observation value) and an angular velocity signal offset calculated with the dead reckoning calculating unit 12 and then inputs the corrected state quantity to the dead reckoning calculating unit 12 (step S110). In step S110, third correction processing is executed using a third Kalman filter observation matrix H3.
In step S109, if the frequency is switched to the third frequency, the Kalman filter correcting unit 14 carries out the correction processing in step S108 as well as corrects an angular velocity offset based on a difference between a gyroscope output signal regarding an angular velocity and an angular velocity signal offset calculated with the dead reckoning calculating unit 12 (step S111). In step S111, fourth correction processing is executed using a fourth Kalman filter observation matrix H4.
The above description is focused on the basic system having six degrees of freedom with the triaxial gyroscope. This system includes many sensors and thus is costly. In view of this, the position detecting unit in FIG. 15 is also applicable to a configuration with fewer sensors. FIGS. 20A to 21C are explanatory views showing various types of sensor configuration. FIGS. 20A and 20B are explanatory views of a sensor coordinate system and a basic system having six degrees of freedom similar to those in FIGS. 16A and 16B.
FIG. 20C is an explanatory view of the sensor configuration of a system equipped with a triaxial accelerometer and a biaxial gyroscope on the sensor board 10 of FIG. 10A. In this system, the biaxial gyroscope makes detection in two axial directions (Gyro x and Gyro z).
FIG. 20D is an explanatory view of the sensor configuration of a system equipped with a triaxial accelerometer and a monoaxial gyroscope on the sensor board 10 of FIG. 10A. In this system, the monoaxial gyroscope makes detection in one axial direction (Gyro z).
FIG. 21A is an explanatory view of the sensor configuration of a system equipped with a biaxial accelerometer and a monoaxial gyroscope on the sensor board 10 of FIG. 10A. In this system, the biaxial accelerometer makes detection in two axial directions (Acc x and Acc y), and the monoaxial gyroscope makes detection in one axial direction (Gyro z).
FIG. 21B is an explanatory view of the sensor configuration of a system equipped with a monoaxial accelerometer and a monoaxial gyroscope on the sensor board 10 of FIG. 10A. In this system, the monoaxial accelerometer makes detection in one axial direction (Acc x), and the monoaxial gyroscope makes detection in one axial direction (Gyro z).
FIG. 21C is an explanatory view of the sensor configuration of a system equipped with an anaxial accelerometer (unused) and a monoaxial gyroscope on the sensor board 10 of FIG. 10A. In this system, the monoaxial gyroscope makes detection in one axial direction (Gyro z).
The dead reckoning calculating unit 12 in FIG. 15 updates a vehicle state quantity inclusive of current vehicle position, speed, and attitude at a high frequency based on the same algorithm used in the basic system with six degrees of freedom at any time. Thus, a sensor configuration controller (SC controller) 15 identifies the actual sensor configuration of an in-vehicle navigation system to estimate an output value of a sensor not included in the basic system to send the value to the dead reckoning calculating unit 12. For example, used as an output value of a sensor not included in the system is a function value of a function that sets a constant (inclusive of 0), white noise, or an output value of another sensor as a variable.
However, the system having fewer sensors involves a problem of lower accuracy. FIG. 22 is an explanatory view of a correspondence relationship between sensor configuration and accuracy. FIG. 22 shows (1) a bearing error (deg) between a road bearing and an estimated vehicle bearing in the case where a vehicle is stopped in parallel to the road at the top floor of a 10-story spiral parking out of GPS range and (2) a difference in estimated height (m) in the case where a vehicle enters the parking from a ground-floor entrance and drives up to the top floor and then returns to a ground-floor exit. In a system equipped with a triaxial accelerometer and a biaxial gyroscope, the bearing error is 3 degrees, and the height difference is 5 m, while in a system equipped with a monoaxial accelerometer and a monoaxial gyroscope, the bearing error and the height difference are as large as 20 degrees (see FIG. 7B) and −11.6 m (see a curve B in FIG. 8), respectively.